
#include <ESP32_Servo.h>
#include "RL_FPV.h"

Servo servo_27;
volatile int speed_x;
volatile int speed_y;
volatile int speed_w;
volatile int angle;

FPV  fpvSerial(1);//UART1
#include <RL_ESP32_Motor.h>
Motor Esp32_Motor_1(1,12,13);
Motor Esp32_Motor_2(2,14,15);
Motor Esp32_Motor_3(3,16,17);
Motor Esp32_Motor_4(4,18,19);

void setup(){
  servo_27.attach(27,500,2500);
  fpvSerial.begin(19200,SERIAL_8N1,23,-1);//定义默认接收管脚 23号
  Esp32_Motor_1.mcpwm_begin();
  Esp32_Motor_2.mcpwm_begin();
  Esp32_Motor_3.mcpwm_begin();
  Esp32_Motor_4.mcpwm_begin();
  speed_x = 0;
  speed_y = 0;
  speed_w = 0;
  angle = 90;
  servo_27.write(angle);
  delay(0);
}

void loop(){
  if (fpvSerial.FpvReadAll()) {
    if (fpvSerial.FpvReadByte(Joystick_LY) > 128) {
      if (angle < 180) {
        angle = angle + 1;

      }

    }
    if (fpvSerial.FpvReadByte(Joystick_LY) < 128) {
      if (angle > 0) {
        angle = angle - 1;

      }

    }
    servo_27.write(angle);
    delay(0);
    speed_x = (map(fpvSerial.FpvReadByte(Joystick_RX), 1, 255, 100, (-100)));
    speed_y = (map(fpvSerial.FpvReadByte(Joystick_RY), 1, 255, 100, (-100)));
    speed_w = (map(fpvSerial.FpvReadByte(Joystick_LX), 1, 255, 100, (-100)));
    Esp32_Motor_1.Motor_Speed(((speed_y - speed_x) + speed_w));
    Esp32_Motor_2.Motor_Speed(((speed_y + speed_x) * -1 + speed_w));
    Esp32_Motor_3.Motor_Speed(((speed_y - speed_x) * -1 + speed_w));
    Esp32_Motor_4.Motor_Speed(((speed_x + speed_y) + speed_w));

  }
  delay(50);

}